from pr2_python import base
from pr2_python.world_interface import WorldInterface
from pr2_tasks.tasks import Tasks

class Room3():
  def __init__(self):
     wi = WorldInterface()

  def goto_table(self):
    b = base.Base()
    try:
      b.move_manipulable_pose(-1.1, 0.2, 0.75, group='torso', try_hard=True)
    except:
      print "unable to move to table"

  def goto_dirty_table(self):
    b = base.Base()
    try:
      b.move_manipulable_pose(1.8, -0.8, 0.75, group='torso', try_hard=True) 
    except:
      print "unable to move to dirty table"

  def goto_shelf(self):
    b = base.Base()
    try:
      b.move_manipulable_pose(1.7, 2.0, 1, group='torso', try_hard=True) 
    except:
      print "unable to move to shelf"

  def move_to_table(self):
     b = base.Base()
     b.move_to_look(-1.1, 0.2, 0.75) 

  def move_to_dirty_table(self):
     b = base.Base()
     b.move_to_look(1.7, -0.5, 0.75)

  def move_to_shelf(self):
     b = base.Base()
     b.move_to_look(1.7, 2.0, 1) 

  def move_arms_back(self):
     tasks = Tasks() 
     tasks.move_arms_to_side()

